#include "Quaternion.h"
#include "Rotation.h"
#include "Vect3.h"

#include <cmath>

using namespace RSIM;

////////////////////////////////////////////////////////////////////////

Quaternion::Quaternion():Vect4(0){data_[0]=1.0;}

////////////////////////////////////////////////////////////////////////

Quaternion::Quaternion(const Vect4& q):Vect4(q){}

////////////////////////////////////////////////////////////////////////

Quaternion::Quaternion(const double& e0, 
                const double& e1, 
                const double& e2, 
                const double& e3):
                Vect4(e0,e1,e2,e3){
                }

////////////////////////////////////////////////////////////////////////

void Quaternion::normalize(){
        double den = sqrt((data_[0]*data_[0] + data_[1]*data_[1] + data_[2]*data_[2] + data_[3]*data_[3]));
        data_[0] /= den; data_[1] /= den; data_[2] /= den; data_[3] /= den;
}

////////////////////////////////////////////////////////////////////////

void Quaternion::calcQDotFromAngularVelocity(   const double *U, 
                                                const double *Q,
                                                double *QDot){
       
        QDot[0] = 0.5*(-Q[1]*U[0] - Q[2]*U[1] - Q[3]*U[2]);
        QDot[1] = 0.5*( Q[0]*U[0] + Q[3]*U[1] - Q[2]*U[2]);
        QDot[2] = 0.5*(-Q[3]*U[0] + Q[0]*U[1] + Q[1]*U[2]);
        QDot[3] = 0.5*( Q[2]*U[0] - Q[1]*U[1] + Q[0]*U[2]);
}

////////////////////////////////////////////////////////////////////////

void Quaternion::calcQDotFromAngularVelocity(   const double *U, 
                                                double *QDot)const{
        // Just a new label for the data array
        const double *Q = this->data_;
        QDot[0] = 0.5*(-Q[1]*U[0] - Q[2]*U[1] - Q[3]*U[2]);
        QDot[1] = 0.5*( Q[0]*U[0] + Q[3]*U[1] - Q[2]*U[2]);
        QDot[2] = 0.5*(-Q[3]*U[0] + Q[0]*U[1] + Q[1]*U[2]);
        QDot[3] = 0.5*( Q[2]*U[0] - Q[1]*U[1] + Q[0]*U[2]);
}
                                        
////////////////////////////////////////////////////////////////////////

void Quaternion::calcAngularVelocityFromEDot(   const double *e,
                                                const double *edot,
                                                double *w){
        
        w[0] = 2*(-e[1]*edot[0] + e[0]*edot[1] - e[3]*edot[2] + e[2]*edot[3]);
        w[1] = 2*(-e[2]*edot[0] + e[3]*edot[1] + e[0]*edot[2] - e[1]*edot[3]);
        w[2] = 2*(-e[3]*edot[0] - e[2]*edot[1] + e[1]*edot[2] + e[0]*edot[3]);
}
                                                        
////////////////////////////////////////////////////////////////////////

void Quaternion::calcQuaternionFromEulerAngles( const double& ang1,
                                                const double& ang2,
                                                const double& ang3,
                                                const char *ch,
                                                double *dat){
        Rotation Rot;
        if(!strcmp(ch,"Body123")){
                Rot.setRotationBody123(Vect3(ang1,ang2,ang3));
        }
        else if(!strcmp(ch,"Space123")){
                Rot.setRotationSpace123(Vect3(ang1,ang2,ang3));
        }
        else{
                cout<<"\nQuaternion::calcQuaternionFromEulerAngles(), "<<
                "unexpected argument. options are \"Body123\" or \"Space123\""<<endl;
                exit(0);
        }
        
        const double *RotMat = Rot.Ptr();
        
        // If trace(Rot)>0, then |e0| > 1/2. So no numerical issues for this case.      
        if( RotMat[0] + RotMat[4] + RotMat[8] > 0.0){
                dat[0] = 0.5*sqrt(RotMat[0]+RotMat[4]+RotMat[8]+1.0); // e0
                double tmp = 0.25/dat[0];
                dat[1] = (RotMat[7]-RotMat[5])*tmp; // e1
                dat[2] = (RotMat[2]-RotMat[6])*tmp; // e2
                dat[3] = (RotMat[3]-RotMat[1])*tmp; // e3
        }
        // |e1|>|e2| and |e1|>|e3|
        else if( (RotMat[0]>RotMat[4]) && (RotMat[0]>RotMat[8]) ){
                dat[1] = 0.5*sqrt(RotMat[0]-RotMat[4]-RotMat[8]+1.0); // e1
                double tmp = 0.25/dat[1];
                dat[0] = (RotMat[7]-RotMat[5])*tmp; // e0
                dat[2] = (RotMat[1]+RotMat[3])*tmp; // e2
                dat[3] = (RotMat[2]+RotMat[6])*tmp; // e3
        }
        // |e2|>|e1| and |e2|>|e3|
        else if(RotMat[4] > RotMat[8]){
                dat[2] = 0.5*sqrt(RotMat[4]-RotMat[0]-RotMat[8]+1.0); // e2
                double tmp = 0.25/dat[2];
                dat[0] = (RotMat[2]-RotMat[6])*tmp; // e0
                dat[1] = (RotMat[1]+RotMat[3])*tmp; // e1
                dat[3] = (RotMat[5]+RotMat[7])*tmp; // e3
        }
        // |e3|>|e1| and |e3|>|e2|
        else{
                dat[3] = 0.5*sqrt(RotMat[8]-RotMat[0]-RotMat[4]+1.0); // e3
                double tmp = 0.25/dat[3];
                dat[0] = (RotMat[3]-RotMat[1])*tmp; // e0
                dat[1] = (RotMat[2]+RotMat[6])*tmp; // e1
                dat[2] = (RotMat[5]+RotMat[7])*tmp; // e2
        }
        
        double den = sqrt((dat[0]*dat[0] + dat[1]*dat[1] + dat[2]*dat[2] + dat[3]*dat[3]));
        dat[0] /= den; dat[1] /= den; dat[2] /= den; dat[3] /= den;
        
}

////////////////////////////////////////////////////////////////////////

Quaternion::Quaternion(const Rotation& Rot):Vect4(0){
        
        const double *RotMat = Rot.Ptr();
        
        // If trace(Rot)>0, then |e0| > 1/2. So no numerical issues for this case.	
        if( RotMat[0] + RotMat[4] + RotMat[8] > 0.0){
                data_[0] = 0.5*sqrt(RotMat[0]+RotMat[4]+RotMat[8]+1.0); // e0
                double tmp = 0.25/data_[0];
                data_[1] = (RotMat[7]-RotMat[5])*tmp; // e1
                data_[2] = (RotMat[2]-RotMat[6])*tmp; // e2
                data_[3] = (RotMat[3]-RotMat[1])*tmp; // e3
        }
        // |e1|>|e2| and |e1|>|e3|
        else if( (RotMat[0]>RotMat[4]) && (RotMat[0]>RotMat[8]) ){
                data_[1] = 0.5*sqrt(RotMat[0]-RotMat[4]-RotMat[8]+1.0); // e1
                double tmp = 0.25/data_[1];
                data_[0] = (RotMat[7]-RotMat[5])*tmp; // e0
                data_[2] = (RotMat[1]+RotMat[3])*tmp; // e2
                data_[3] = (RotMat[2]+RotMat[6])*tmp; // e3
        }
        // |e2|>|e1| and |e2|>|e3|
        else if(RotMat[4] > RotMat[8]){
                data_[2] = 0.5*sqrt(RotMat[4]-RotMat[0]-RotMat[8]+1.0); // e2
                double tmp = 0.25/data_[2];
                data_[0] = (RotMat[2]-RotMat[6])*tmp; // e0
                data_[1] = (RotMat[1]+RotMat[3])*tmp; // e1
                data_[3] = (RotMat[5]+RotMat[7])*tmp; // e3
        }
        // |e3|>|e1| and |e3|>|e2|
        else{
                data_[3] = 0.5*sqrt(RotMat[8]-RotMat[0]-RotMat[4]+1.0); // e3
                double tmp = 0.25/data_[3];
                data_[0] = (RotMat[3]-RotMat[1])*tmp; // e0
                data_[1] = (RotMat[2]+RotMat[6])*tmp; // e1
                data_[2] = (RotMat[5]+RotMat[7])*tmp; // e2
        }
        
        // kdb:Is this required here ?
        this->normalize();
}

////////////////////////////////////////////////////////////////////////
// Following is an interesting implementation I came across written by 
// J.M.P. van Waveren, of I.D Software. The idea is to avoid
// using the expensive sqrt function. There is a smart method to compute
// reciprocal square root method (See http://en.wikipedia.org/wiki/Fast_inverse_square_root).
// if(trace(RotMat))>0, then e0 = 0.5*reciprocalsquareroot(trace+1)*(trace+1) !
// This would be much faster than the current implementation but not as
// accurate.
////////////////////////////////////////////////////////////////////////